#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Rough_tracking_calculation.h>
#include <SoftwareSerial.h>
#include <Ticker.h>
#include <TinyGPSPlus.h>
#include <DataParser.hpp>
#include <FifoBuffer.hpp>

#include <BLE2902.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <EEPROM.h>
#include <queue>
#define STEPPER1_DIR_PIN       10
#define STEPPER1_STEP_PIN      6
#define STEPPER2_DIR_PIN       8
#define STEPPER2_STEP_PIN      5
#define DEBOUNCE_DELAY         (100)
#define LED_PIN                13
#define SUN_DATA_FILTER_SIZE   5
#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
int x_offset, y_offset;

struct BLE {
};

BLEServer* pServer = NULL; // BLE Server
BLECharacteristic* pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;
std::queue<uint8_t> rxQueue; // 接收队列
int findIndex = 0;
int param_len = 0;

struct ExtInterrupt {
    bool state;
    uint8_t pin;
    unsigned long lastTime;
};

struct StepperPair {
    AccelStepper* stepper;
    ExtInterrupt* limitSwitch;
};

ExtInterrupt limitSwitch1 = {false, 3, 0};
ExtInterrupt limitSwitch2 = {false, 7, 0};
AccelStepper stepper1(AccelStepper::DRIVER, STEPPER1_STEP_PIN, STEPPER1_DIR_PIN); // 配置电机1 的脉冲引脚 方向引脚
AccelStepper stepper2(AccelStepper::DRIVER, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN); // 配置电机2 的脉冲引脚 方向引脚
SerialDataParser dataParser;
FIFOBuffer<SunData, SUN_DATA_FILTER_SIZE> sunDataQueue;
StepperPair stepperPairs[2] = {
    {&stepper1, &limitSwitch1},
    {&stepper2, &limitSwitch2}
};

double elevatingAngle = 0; // 高度角
double azimuthAngle = 0;   // 方位角
uint8_t led;
int GPStime;

SoftwareSerial videoSerial(0, -1);
Ticker timer; // 定时器
TinyGPSPlus gps;

// 定义状态枚举
enum State {
    STATE_Roughtracking, // 粗跟踪状态
    STATE_Finetracking,  // 细跟踪状态
    STATE_Reseting,      // 复位状态
    STATE_NULL,          // 空状态
    STATE_BLE            // 蓝牙状态
};
State currentState = STATE_NULL;

void DisplayInfo() {
    Serial.print("Location:");
    if (gps.location.isValid()) {
        Serial.print(gps.location.lat(), 6);
        Serial.print(",");
        Serial.print(gps.location.lng(), 6);
    } else {
        Serial.print("INVALID");
    }

    Serial.print("  Date");
    if (gps.date.isValid()) {
        Serial.print(gps.time.hour());
        Serial.print("/");
        Serial.print(gps.time.minute());
        Serial.print("/");
        Serial.print(gps.time.second());
    } else {
        Serial.print("INVALID");
    }
    Serial.println();
}

void CalculateSunAngle() {
    double LatitudeData = gps.location.lat();
    double LongitudeData = gps.location.lng();
    double hourdata = gps.time.hour() + (gps.time.minute() / 60.0) + (gps.time.second() / 3600.0);

    // 计算高度角和方位角
    elevatingAngle = Height_Angle_Calculation(LongitudeData, LatitudeData, gps.date.year(), gps.date.month(), gps.date.day(), hourdata);
    azimuthAngle = Azimuth_Angle_Calculation(LongitudeData, LatitudeData, gps.date.year(), gps.date.month(), gps.date.day(), hourdata); // 以正北为0度，顺时针为正，逆时针为负
    Serial.print("Hour.");
    Serial.println(hourdata);
    Serial.print("elevatingAngle.");
    Serial.println(elevatingAngle);
    Serial.print("azimuthAngle.");
    Serial.println(azimuthAngle);
}

void CoarseTracking() {
    RotationAngle(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360); // 设置电机目标角度
    RotationAngle(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);
    // RotationAngle(stepper1, -4000); // 设置电机目标角度
    // RotationAngle(stepper2, -4000);
    // Serial.print("stepper1.currentPosition().");
    // Serial.println(stepper1.currentPosition());
    // Serial.print("stepper2.currentPosition().");
    // Serial.println(stepper2.currentPosition());
    // Serial.print("stepper1.distanceToGo().");
    // Serial.println(stepper1.distanceToGo());
    // Serial.print("stepper2.distanceToGo().");
    // Serial.println(stepper2.distanceToGo());
}

// Debug调试功能，正式版本上不启用，代码review时也无需关注
void RunUsrCmd(uint8_t cmd, int data) {
    int stepperNum; // 控制电机编号
    int dataflag;
    switch (cmd) {
    case 'o': // 利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息

        Serial.print(F("stepper1 Position: "));
        Serial.println(stepper1.currentPosition());
        Serial.print(F("stepper2 Position: "));
        Serial.println(stepper2.currentPosition());
        Serial.print(F("Current Running Motor: "));

        if (stepperNum == 1 || stepperNum == 2) {
            Serial.print(F("Motor# "));
            Serial.println(stepperNum);
        } else if (stepperNum == 0) {
            Serial.println(F("Both Motors"));
        }
        break;

    case 'v': // 利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。

        if (stepperNum == 1) {
            Serial.print(F("Motor1 'moveTo' "));
            Serial.println(data);
            stepper1.moveTo(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Motor2 'moveTo' "));
            Serial.println(data);
            stepper2.moveTo(data);
        } else if (stepperNum == 0) {
            Serial.print(F("Both Motors 'moveTo' "));
            Serial.println(data);
            stepper1.moveTo(data);
            stepper2.moveTo(data);
        }
        break;

    case 'm': // 利用move函数使电机运行相应步数。move函数不会“block”程序。

        if (stepperNum == 1) {
            Serial.print(F("Motor1 'move'  "));
            Serial.println(data);
            stepper1.move(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Motor2 'move'  "));
            Serial.println(data);
            stepper2.move(data);
        } else if (stepperNum == 0) {
            Serial.print(F("Both Motors 'move'  "));
            Serial.println(data);
            if (data <= 0) {
                dataflag = true;
            } else
                dataflag = false;
            stepper1.move(data);
            stepper2.move(data);
        }
        break;

    case 'r': // 利用runToNewPosition函数让电机运行到用户指定位置值。
        if (stepperNum == 1) {
            Serial.print(F("Motor1 'runToNewPosition' "));
            Serial.println(data);
            stepper1.runToNewPosition(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Motor2 'runToNewPosition' "));
            Serial.println(data);
            stepper2.runToNewPosition(data);
        } else if (stepperNum == 0) {
            Serial.print(F("Both Motors 'runToNewPosition' "));
            Serial.println(data);
            stepper1.runToNewPosition(data);
            stepper2.runToNewPosition(data);
        }
        break;

    case 's': // 利用setCurrentPosition函数设置当前位置为用户指定位置值。
        if (stepperNum == 1) {
            Serial.print(F("Set stepper1 Current Position to "));
            Serial.println(data);
            stepper1.setCurrentPosition(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Set stepper2 Current Position to "));
            Serial.println(data);
            stepper2.setCurrentPosition(data);
        } else if (stepperNum == 0) {
            stepper1.moveTo(data);
            Serial.print(F("Set both steppers' Current Position to "));
            Serial.println(data);
            stepper1.setCurrentPosition(data);
            stepper2.setCurrentPosition(data);
        }
        break;

    case 'a': // 利用setAcceleration函数设置加速度
        if (stepperNum == 1) {
            Serial.print(F("Motor1 'setAcceleration' "));
            Serial.println(data);
            stepper1.setAcceleration(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Motor2 'setAcceleration' "));
            Serial.println(data);
            stepper2.setAcceleration(data);
        } else if (stepperNum == 0) {
            Serial.print(F("Both Motors 'setAcceleration' "));
            Serial.println(data);
            stepper1.setAcceleration(data);
            stepper2.setAcceleration(data);
        }
        break;

    case 'x': // 利用setMaxSpeed函数设置最大速度
        if (stepperNum == 1) {
            Serial.print(F("Motor1 'setMaxSpeed' "));
            Serial.println(data);
            stepper1.setMaxSpeed(data);
        } else if (stepperNum == 2) {
            Serial.print(F("Motor2 'setMaxSpeed' "));
            Serial.println(data);
            stepper2.setMaxSpeed(data);
        } else if (stepperNum == 0) {
            Serial.print(F("Both Motors 'setMaxSpeed' "));
            Serial.println(data);
            stepper1.setMaxSpeed(data);
            stepper2.setMaxSpeed(data);
        }
        break;

    case 'd': // 用户通过此指令可指定哪一个电机进行工作
        if (data == 1 || data == 2) {
            stepperNum = data;
            Serial.print(F("Running Motor "));
            Serial.println(stepperNum);
        } else if (data == 0) {
            stepperNum = data;
            Serial.println(F("Running Both Motors "));
        } else {
            Serial.print(F("Motor Number Wrong."));
        }
        break;
    case 'g':
        //  onSerial1Data();
        //  timer.attach(15*60, onSerial1Data); // 设置定时器1的回调函数，每隔30s触发一次
        break;
    case 'j':
        DisplayInfo();
        break;
    case 't':
        CalculateSunAngle();
        break;
    default: // 未知指令
        Serial.println(F("Unknown Command"));
    }
}

void FineTracking(u16_t filterX, u16_t filterY) {
    x_offset = (120 - filterX); // 计算偏移量
    y_offset = (120 - filterY);
    if (abs(x_offset) < 10 && abs(y_offset) < 10) { // 偏移量小于15时，停止电机运行
        stepper1.setCurrentPosition(-azimuthAngle * CIRCLE_Puls1 / 360);
        stepper2.setCurrentPosition(-elevatingAngle * CIRCLE_Puls2 / 360);
    } else {
        stepper1.move(x_offset * 2);
        stepper2.move(y_offset * 2);
    }
    // Serial.print("filterX: ");
    // Serial.println(filterX);
    // Serial.print("filterY: ");
    // Serial.println(filterY);
    Serial.print("x_offset: ");
    Serial.println(x_offset);
    Serial.print("y_offset: ");
    Serial.println(y_offset);
}

void onSerial1Data() {
    unsigned long startTime = millis();
    while (Serial1.available() && (millis() - startTime < 1000)) { // 最多等待1秒
        Serial1.read();
    }

    while (Serial1.available() <= 0)
        ; // 等待数据到来
    // 持续读取并更新GPS数据
    startTime = millis();
    while (Serial1.available() > 0 || (millis() - startTime < 1000)) {
        gps.encode(Serial1.read());
        // 检查GPS数据是否有效
        if (gps.time.isUpdated() && gps.location.isUpdated()) {
            break;
        }
    }
    DisplayInfo();
    CalculateSunAngle();
    if (elevatingAngle <= 10) { // 太阳落山
        currentState = STATE_Reseting;
    }
    Serial.print("currentState: ");
    Serial.println(currentState);
    GPStime++;
}

void LimitswitchstepperInterrupt(void* arg) // 中断函数，当电机1的限位开关被触发时，停止电机运行
{
    auto limitSwitch = static_cast<ExtInterrupt*>(arg);
    limitSwitch->lastTime = millis();
    limitSwitch->state = true;
}
class MyServerCallbacks : public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
        deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
        deviceConnected = false;
    }
};
class MyCallbacks : public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic* pCharacteristic) {
        std::string rxValue = pCharacteristic->getValue();
        if (rxValue.length() > 0) // 接收到数据且数据为0xEE
        {
            // rxQueue.push(rxValue.length()); // 发送数据长度
            for (int i = 0; i < rxValue.length(); i++) // 将接收到的数据拷贝到接收队列中
            {
                rxQueue.push(rxValue[i]);
                if (rxQueue.front() != 0xEE && findIndex == 0) {
                    rxQueue.pop(); // 弹出队列
                    continue;
                } else if (rxQueue.front() == 0xEE)
                    findIndex = 1;
                if (rxQueue.back() == 0xFF && findIndex == 1)
                    findIndex = 2;
            }
        }
    }
};
void controlStepper() {

    if (findIndex == 2 && !rxQueue.empty()) {             // 接收队列不为空
        uint8_t rxBufTmp[50] = {0}, len = rxQueue.size(); // 定义接收队列临时变量，最大长度为50
        uint8_t sumcheck = 0;                             // 校验和

        for (size_t i = 0; i < len; i++) // 将接收队列内容拷贝到临时变量中
        {
            rxBufTmp[i] = rxQueue.front();
            rxQueue.pop(); // 弹出队列
            if (i > 0 && i < len - 2) {
                sumcheck += rxBufTmp[i];
            }
        }
        if (rxBufTmp[len - 2] != sumcheck) {
            findIndex = 0;
            return;
        }
        // do {
        // if (len != rxBufTmp[2] + 4)

        // pTxCharacteristic->setValue(rxBufTmp, len);  //发送rxvalue内容，字节长度为rxQueue.size()
        if (rxBufTmp[0] == 0xEE && rxBufTmp[len - 1] == 0xFF) // 接收到数据且数据为0xEE
                                                              //   pTxCharacteristic->setValue(rxBufTmp,len);
        {
            uint8_t buf[16] = {0};

            // Serial.write(rxBufTmp,len);
            pTxCharacteristic->setValue(rxBufTmp, len);

            switch (rxBufTmp[1]) {
            case 0x01: // 控制俯仰角电机上转
                switch (rxBufTmp[2]) {
                case 0x00:                // 转360度，模拟一直转 EE 01 00 FF
                    elevatingAngle = 360; // 高度角
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                    break;
                case 0x01:                        // 读取俯仰角，转动电机，最大255度
                    elevatingAngle = rxBufTmp[3]; // 高度角
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                    break;
                case 0x02:                                 // 读取俯仰角，转动电机,可设置到360度
                    elevatingAngle = -rxBufTmp[4];         // 高度角低位
                    elevatingAngle += -(rxBufTmp[3] << 8); // 高度角高位
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                default:

                    break;
                }
                // AzimuthAngle = -90; //方位角
                break;
            case 0x02:
                switch (rxBufTmp[2]) {
                case 0x00:                 // 转360度，模拟一直转
                    elevatingAngle = -360; // 高度角
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                    break;
                case 0x01:                         // 读取俯仰角，转动电机，最大255度
                    elevatingAngle = -rxBufTmp[3]; // 高度角
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                    break;
                case 0x02:                                 // 读取俯仰角，转动电机,可设置到360度
                    elevatingAngle = -rxBufTmp[4];         // 高度角低位
                    elevatingAngle += -(rxBufTmp[3] << 8); // 高度角高位
                    Rotation_BLE(stepper2, -elevatingAngle * CIRCLE_Puls2 / 360);

                default:

                    break;
                }

                break;
            case 0x03: // 顺时针转动
                switch (rxBufTmp[2]) {
                case 0x00:              // 转360度，模拟一直转
                    azimuthAngle = 360; // 高度角
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                    break;
                case 0x01:                      // 读取俯仰角，转动电机，最大255度
                    azimuthAngle = rxBufTmp[3]; // 高度角
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                    break;
                case 0x02:                              // 读取俯仰角，转动电机,可设置到360度
                    azimuthAngle = rxBufTmp[4];         // 高度角低位
                    azimuthAngle += (rxBufTmp[3] << 8); // 高度角高位
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                default:

                    break;
                }
                break;

            case 0x04:
                switch (rxBufTmp[2]) {
                case 0x00:               // 转360度，模拟一直转
                    azimuthAngle = -360; // 高度角
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                    break;
                case 0x01:                       // 读取俯仰角，转动电机，最大255度
                    azimuthAngle = -rxBufTmp[3]; // 高度角
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                    break;
                case 0x02:                               // 读取俯仰角，转动电机,可设置到360度
                    azimuthAngle = -rxBufTmp[4];         // 高度角低位
                    azimuthAngle += -(rxBufTmp[3] << 8); // 高度角高位
                    Rotation_BLE(stepper1, -azimuthAngle * CIRCLE_Puls1 / 360);

                default:

                    break;
                }
                break;

            case 0x05:
                for (size_t i = 0; i < 2; i++) {
                    auto stepperPair = &stepperPairs[i];
                    stepperPair->stepper->stop();
                }
                break;

            case 0x06:

                break;

            case 0x07:

                break;

            case 0x08: // 修改参数、保存参数
                // EE 08 00 0x FF 读数据

                if (rxBufTmp[2] == 0x00) {

                    param_len = EEPROM.read(rxBufTmp[3] * 16);
                    if (param_len == 0xff) {
                        buf[0] = 0x4e;
                        buf[1] = 0x4f;
                        buf[2] = 0x4e;
                        buf[3] = 0x45;
                        pTxCharacteristic->setValue(buf, 4);
                        break;
                    }

                    for (int i = 0; i < param_len; i++) {
                        buf[i] = EEPROM.read(rxBufTmp[3] * 16 + i + 1);
                    }
                    pTxCharacteristic->setValue(buf, param_len);
                    Serial.write(buf, param_len);
                }

                else if (rxBufTmp[2] <= 0x08) // EE 08 0x 04 xx xx xx xx FF  写数据
                {
                    param_len = rxBufTmp[3]; // 参数长度
                    EEPROM.write(rxBufTmp[2] * 16, rxBufTmp[3]);

                    for (int i = 0; i < param_len; i++) {
                        EEPROM.write(rxBufTmp[2] * 16 + i + 1, rxBufTmp[4 + i]);
                        delay(1);
                    }
                    EEPROM.commit();

                    param_len = EEPROM.read(rxBufTmp[2] * 16);

                    for (int i = 0; i < param_len; i++) {
                        buf[i] = EEPROM.read(rxBufTmp[2] * 16 + i + 1);
                    }
                    pTxCharacteristic->setValue(buf, param_len);
                    Serial.write(buf, param_len);
                } else
                    break;

                break;

            case 0x09:

                break;

            default:

                break;
            }
        }
        // } while (0);
        pTxCharacteristic->notify();
        findIndex = 0;
    }
    return;
}
// 初始化代码
void setup() {
    pinMode(LED_PIN, OUTPUT);
    Serial.begin(115200);
    Serial1.begin(115200, SERIAL_8N1, 1, -1);
    videoSerial.begin(115200);
    for (auto& stepperPair : stepperPairs) {
        pinMode(stepperPair.limitSwitch->pin, INPUT_PULLUP);                                                                                    // 限位开关初始化
        attachInterruptArg(digitalPinToInterrupt(stepperPair.limitSwitch->pin), LimitswitchstepperInterrupt, stepperPair.limitSwitch, FALLING); // 限位开关中断初始化
        stepper_init(*stepperPair.stepper, 1000, 250);
    }
    stepper1.setPinsInverted(true);
    currentState = STATE_Reseting;
    timer.attach(10, onSerial1Data); // 设置定时器1的回调函数，每分钟触发一次
    EEPROM.begin(144);               // 初始化EEPROM
    EEPROM.write(0, 0XFF);           // 将第一个字节写入0XFF,作为rom是否初始化标识符
    if (EEPROM.read(0) != 0XFF) {    // 如果rom未初始化,则初始化
        for (int i = 0; i < 144; i++) {
            EEPROM.write(i, 0xFF); // 将所有字节写入0xFF
        }
        EEPROM.commit(); // 保存数据到EEPROM
    }
    //////////////////蓝牙模块初始化//////////////////
    // Create the BLE Device
    BLEDevice::init("sunTracker"); // 初始化蓝牙设备,设置名称
    // Create the BLE Server
    pServer = BLEDevice::createServer();            // 创建蓝牙服务端
    pServer->setCallbacks(new MyServerCallbacks()); // 设置服务端回调函数
    // Create the BLE Service
    BLEService* pService = pServer->createService(SERVICE_UUID); // 创建蓝牙服务,设置UUID
    // Create a BLE Characteristic
    pTxCharacteristic = pService->createCharacteristic(
        CHARACTERISTIC_UUID_TX,
        BLECharacteristic::PROPERTY_NOTIFY); // 创建蓝牙特征,设置UUID,属性为通知
    pTxCharacteristic->addDescriptor(new BLE2902());
    BLECharacteristic* pRxCharacteristic = pService->createCharacteristic(
        CHARACTERISTIC_UUID_RX,
        BLECharacteristic::PROPERTY_WRITE);             // 创建蓝牙特征,设置UUID,属性为写
    pRxCharacteristic->setCallbacks(new MyCallbacks()); // 设置特征回调函数
    // Start the service
    pService->start();                  // 启动服务
    pServer->getAdvertising()->start(); // 启动广播
}

// 主循环
void loop() {
    if (deviceConnected) //
    {
        if (currentState != STATE_BLE) {
            stepper1.stop();
            stepper2.stop();
        }
        currentState = STATE_BLE;
        controlStepper();
    }
    if (!deviceConnected && oldDeviceConnected) {
        delay(500);                  // give the bluetooth stack the chance to get things ready
        pServer->startAdvertising(); // restart advertising
        Serial.println("start advertising");
        oldDeviceConnected = deviceConnected;
        currentState = STATE_Reseting;
    }
    // connecting
    if (deviceConnected && !oldDeviceConnected) {
        // do stuff here on connecting
        oldDeviceConnected = deviceConnected;
    }

    SunData curSunData;
    for (size_t i = 0; i < 2; i++) {
        auto stepperPair = &stepperPairs[i];
        if (stepperPair->limitSwitch->state && (millis() - stepperPair->limitSwitch->lastTime > DEBOUNCE_DELAY)) { // 处理电机限位开关被触发后的事件
            if (digitalRead(stepperPair->limitSwitch->pin) == LOW) {
                stepperPair->stepper->stop();
                stepperPair->stepper->setCurrentPosition(0);
                Serial.print(F("reset: "));
                Serial.println(i);
            }
            stepperPair->limitSwitch->state = false;
        }
    }

    // 处理两个限位开关同时触发的事件
    if (digitalRead(limitSwitch1.pin) == LOW && digitalRead(limitSwitch2.pin) == LOW) {
        // Serial.print("elevatingAngle");
        // Serial.println(elevatingAngle);
        if (elevatingAngle > 10) {
            Serial.println(F("STATE_Roughtracking"));
            if (currentState != STATE_BLE) {
                currentState = STATE_Roughtracking;
            }
        } else {
            currentState = STATE_NULL;
        }
    }

    switch (currentState) // 状态切换
    {
    case STATE_Roughtracking: // 粗跟踪状态
        // todo 设置正确的GPS数据更新次数
        if (GPStime >= 1) // 视频板没有捕捉到太阳,且GPS数据更新了10次
        {
            Serial.println(F("Start CoarseTracking"));
            CoarseTracking(); // 粗跟踪
            GPStime = 0;
            if (abs(stepper1.distanceToGo()) < 500 && abs(stepper2.distanceToGo()) < 500) {
                Serial.println(F("STATE_Finetracking"));
                currentState = STATE_Finetracking;
            }
        }
        break;
    case STATE_Finetracking:           // 细跟踪状态
        if (videoSerial.available()) { // 视频板有数据
            auto data = videoSerial.read();
            Serial.print(F("videoSerial.read() = "));
            Serial.println(data);
            if (dataParser.parseData(data)) {
                Serial.println(__LINE__);
                if (dataParser.getCmd() == CMD_SUN_DATA) { // 视频板数据帧格式：HEADER, CMD_SUN_DATA, LEN, x, y, graySun, grayBg, checksum, END
                    Serial.println(__LINE__);
                    SunData sunData;
                    if (dataParser.getPacketData<SunData>(sunData)) {
                        Serial.println(__LINE__);
                        if (sunDataQueue.full()) {
                            sunDataQueue.pop();
                            Serial.println(F("sunDataQueue is full"));
                        }
                        sunDataQueue.push(sunData);
                        // Serial.print(F("SunDataQueue.size() = "));
                        // Serial.print(sunDataQueue.size());
                        // Serial.print(F(", x = "));
                        // Serial.print(sunData.x);
                        // Serial.print(F(", y = "));
                        // Serial.print(sunData.y);
                        // Serial.print(F(", graySun = "));
                        // Serial.print(sunData.graySun);
                        // Serial.print(F(", grayBg = "));
                        // Serial.print(sunData.grayBg);
                        // Serial.println(F(""));
                        curSunData = sunDataQueue.back(); // 最新收到的太阳数据
                        static bool sunDatavalid = false;
                        // Serial.print(F("sunDataQueue.size() = "));
                        // Serial.println(sunDataQueue.size());
                        // 判断太阳数据是否可用
                        if (sunDataQueue.full()) {
                            for (size_t i = 0; i < sunDataQueue.size(); i++) {
                                auto sunData = sunDataQueue.at(i);
                                static uint8_t loseTargetCnt = 0;
                                if (abs(sunData.x - curSunData.x) > 50 || abs(sunData.y - curSunData.y) > 50 || (curSunData.graySun - curSunData.grayBg) < 100) { // 是否丢失目标
                                    // Serial.print(F("curSunData.x = "));
                                    // Serial.print(curSunData.x);
                                    // Serial.print(F(", curSunData.y = "));
                                    // Serial.print(curSunData.y);
                                    // Serial.print(F(", sunData.x = "));
                                    // Serial.print(sunData.x);
                                    // Serial.print(F(", sunData.y = "));
                                    // Serial.print(sunData.y);
                                    // Serial.print(F(", (curSunData.graySun - curSunData.grayBg) = "));
                                    // Serial.print(curSunData.graySun - curSunData.grayBg);
                                    // Serial.println(F(""));
                                    if (loseTargetCnt++ >= 10) {
                                        sunDataQueue.clear();
                                        currentState = STATE_Roughtracking;
                                        loseTargetCnt = 0;
                                    }
                                    sunDatavalid = false;
                                    break;
                                } else {
                                    sunDatavalid = true;
                                    loseTargetCnt = 0;
                                }
                            }
                            Serial.print(F("sunDatavalid = "));
                            Serial.println(sunDatavalid);
                            if (sunDatavalid) {
                                FineTracking(curSunData.x, curSunData.y); // 控制电机进行精跟踪
                            }
                        }
                    }
                    dataParser.reset();
                }
            }
        }

        break;

    case STATE_Reseting:
        for (auto& stepperPair : stepperPairs) {
            if (digitalRead(stepperPair.limitSwitch->pin) == HIGH) {
                stepperPair.stepper->move(160000);
            }
        }
        currentState = STATE_NULL;
        break;
    default:
        break;
    }

    if (!(digitalRead(limitSwitch1.pin) == 0 && (azimuthAngle < 0 || x_offset < 0))) {
        stepper1.run();
    }
    if ((!(digitalRead(limitSwitch2.pin) == 0 && (elevatingAngle < 10 || y_offset < 0)))) {
        stepper2.run();
    }
}